#include "rt/rt_spi.h"
#include <math.h>

/*
左前髋内外关节：外+内-   左前髋关节屈伸关节：前+后-   左前膝关节关节：上+下-
右前髋内外关节：外-内+   右前髋关节屈伸关节：前+后-   右前膝关节关节：上+下-
左后髋内外关节：外+内-   左后髋关节屈伸关节：前+后-   左后膝关节关节：上+下-
右后髋内外关节：外-内+   右后髋关节屈伸关节：前+后-   右后膝关节关节：上+下-
*/

// 模式三选一
// #define VIEW_MODE// 查看模式，不控制
// #define ZPOSE_MODE// 零位模式，不控制，但会发送设置零位命令
#define TEST_MODE// 测试模式，控制

#define bit(n)                (1UL << (n))                               // 移位置1
#define bitRead(value, n)     (((value) >> (n)) & 0x01)                  // 读位
#define bitSet(value, n)      ((value) |= bit(n))                        // 置位
#define bitClear(value, n)    ((value) &= ~bit(n))                       // 清位
#define bitToggle(value, n)   ((value) ^= bit(n))                        // 反位
#define bitWrite(value, n, b) (b ? bitSet(value, n) : bitClear(value, n))// 写位

#define TOTALSTEPS 3500
#define HOLDSTEPS  500

uint32_t stepCnt = 0;// 测试步骤计数

// @brief 计算下一个角度，变化规律为两端为零，中间最大，线性变化，且在最大或最小处保持一定步数
// @param max 中间最大角度
// @param step 当前步数
// @param totalSteps 总步数（包含保持步数）
// @param holdSteps 最大或最小处保持的步数
// @return 下一个角度
float computeNextAngle(float max, uint32_t step, uint32_t totalSteps, uint32_t holdSteps)
{
    // 计算除去保持步数后的实际变化步数
    uint32_t actualSteps     = totalSteps - 2 * holdSteps;
    uint32_t halfActualSteps = actualSteps / 2;
    uint32_t firstHoldStart  = halfActualSteps;
    uint32_t secondHoldStart = halfActualSteps + holdSteps;

    float angle;

    max = max * 0.1f;

    if(step < firstHoldStart)
    {
        // 前半段，从 0 线性递增
        angle = max * (float)step / (float)halfActualSteps;
    }
    else if(step < firstHoldStart + holdSteps)
    {
        angle = max;
    }
    else if(step < secondHoldStart + halfActualSteps)
    {
        // 后半段，线性递减到 0
        angle = max * (float)(totalSteps - holdSteps - step) / (float)halfActualSteps;
    }
    else
    {
        angle = 0;
    }

    return angle;
}

// @brief 计算下一个角度，变化规律为两端为 ep，中间最大为 mid，线性变化，且在最大处保持一定步数
// @param ep 两端的角度
// @param mid 中间角度
// @param step 当前步数
// @param totalSteps 总步数（包含保持步数）
// @param holdSteps 最大处保持的步数
// @return 下一个角度
float computeNextAngleWithEp(float ep, float mid, uint32_t step, uint32_t totalSteps, uint32_t holdSteps)
{
    // 计算除去保持步数后的实际变化步数
    uint32_t actualSteps     = totalSteps - 2 * holdSteps;
    uint32_t halfActualSteps = actualSteps / 2;
    uint32_t firstHoldStart  = halfActualSteps;
    uint32_t secondHoldStart = halfActualSteps + holdSteps;

    float angle;

    if(step < firstHoldStart)
    {
        // 前半段，从 ep 线性递增到 mid
        angle = ep + (mid - ep) * (float)step / (float)halfActualSteps;
    }
    else if(step < firstHoldStart + holdSteps)
    {
        angle = mid;
    }
    else if(step < secondHoldStart + halfActualSteps)
    {
        // 后半段，从 mid 线性递减到 ep
        angle = mid - (mid - ep) * (float)(step - secondHoldStart) / (float)halfActualSteps;
    }
    else
    {
        angle = ep;
    }

    return angle;
}

void setJointCommand(spi_command_t* cmd, uint8_t legNo, uint8_t jointNo, float a, float w, float t, float kp, float kd)
{
    if(jointNo == 1)
    {
        cmd->q_des_abad[legNo]  = a;
        cmd->qd_des_abad[legNo] = w;
        cmd->tau_abad_ff[legNo] = t;
        cmd->kp_abad[legNo]     = kp;
        cmd->kd_abad[legNo]     = kd;
    }
    else if(jointNo == 2)
    {
        cmd->q_des_hip[legNo]  = a;
        cmd->qd_des_hip[legNo] = w;
        cmd->tau_hip_ff[legNo] = t;
        cmd->kp_hip[legNo]     = kp;
        cmd->kd_hip[legNo]     = kd;
    }
    else if(jointNo == 3)
    {
        cmd->q_des_knee[legNo]  = a;
        cmd->qd_des_knee[legNo] = w;
        cmd->tau_knee_ff[legNo] = t;
        cmd->kp_knee[legNo]     = kp;
        cmd->kd_knee[legNo]     = kd;
    }
}

// @简介 站蹲测试
void standSquatTest(spi_command_t* cmd, spi_data_t* dat)
{
    static bool isNewTest = true;

    float testKp = 500.f;//80.0f;// 500
    float testKd = 1.0f;//0.0f; // 0
    if(isNewTest)
    {
        uint8_t bz[12] = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};

        // 左前髋内外关节
        float da = FRONT_ABAD_OFFSET - dat->q_abad[1];
        float a  = copysign(0.01f, da);
        if(fabs(da) > 0.01f)
        {
            setJointCommand(cmd, 1, 1, dat->q_abad[1] + a, 0, 0, testKp, testKd);
        }
        else
        {
            bz[0] = 0;
        }

        // 左前髋屈伸关节
        da = -FRONT_HIP_OFFSET - dat->q_hip[1];
        a  = copysign(0.01f, da);
        if(fabs(da) > 0.01f)
        {
            setJointCommand(cmd, 1, 2, dat->q_hip[1] + a, 0, 0, testKp, testKd);
        }
        else
        {
            bz[1] = 0;
        }

        // 左前膝关节关节
        da = FRONT_KNEE_OFFSET - dat->q_knee[1];
        a  = copysign(0.01f, da);
        if(fabs(da) > 0.01f)
        {
            setJointCommand(cmd, 1, 3, dat->q_knee[1] + a, 0, 0, testKp, testKd);
        }
        else
        {
            bz[2] = 0;
        }

        // 右前髋内外关节
        da = -FRONT_ABAD_OFFSET - dat->q_abad[0];
        a  = copysign(0.01f, da);
        if(fabs(da) > 0.01f)
        {
            setJointCommand(cmd, 0, 1, dat->q_abad[0] + a, 0, 0, testKp, testKd);
        }
        else
        {
            bz[3] = 0;
        }

        // 右前髋屈伸关节
        da = -FRONT_HIP_OFFSET - dat->q_hip[0];
        a  = copysign(0.01f, da);
        if(fabs(da) > 0.01f)
        {
            setJointCommand(cmd, 0, 2, dat->q_hip[0] + a, 0, 0, testKp, testKd);
        }
        else
        {
            bz[4] = 0;
        }

        // 右前膝关节关节
        da = FRONT_KNEE_OFFSET - dat->q_knee[0];
        a  = copysign(0.01f, da);
        if(fabs(da) > 0.01f)
        {
            setJointCommand(cmd, 0, 3, dat->q_knee[0] + a, 0, 0, testKp, testKd);
        }
        else
        {
            bz[5] = 0;
        }

        // 左后髋内外关节
        da = BACK_ABAD_OFFSET - dat->q_abad[3];
        a  = copysign(0.01f, da);
        if(fabs(da) > 0.01f)
        {
            setJointCommand(cmd, 3, 1, dat->q_abad[3] + a, 0, 0, testKp, testKd);
        }
        else
        {
            bz[6] = 0;
        }

        // 左后髋屈伸关节
        da = -BACK_HIP_OFFSET - dat->q_hip[3];
        a  = copysign(0.01f, da);
        if(fabs(da) > 0.01f)
        {
            setJointCommand(cmd, 3, 2, dat->q_hip[3] + a, 0, 0, testKp, testKd);
        }
        else
        {
            bz[7] = 0;
        }

        // 左后膝关节关节
        da = BACK_KNEE_OFFSET - dat->q_knee[3];
        a  = copysign(0.01f, da);
        if(fabs(da) > 0.01f)
        {
            setJointCommand(cmd, 3, 3, dat->q_knee[3] + a, 0, 0, testKp, testKd);
        }
        else
        {
            bz[8] = 0;
        }

        // 右后髋内外关节
        da = -BACK_ABAD_OFFSET - dat->q_abad[2];
        a  = copysign(0.01f, da);
        if(fabs(da) > 0.01f)
        {
            setJointCommand(cmd, 2, 1, dat->q_abad[2] + a, 0, 0, testKp, testKd);
        }
        else
        {
            bz[9] = 0;
        }

        // 右后髋屈伸关节
        da = -BACK_HIP_OFFSET - dat->q_hip[2];
        a  = copysign(0.01f, da);
        if(fabs(da) > 0.01f)
        {
            setJointCommand(cmd, 2, 2, dat->q_hip[2] + a, 0, 0, testKp, testKd);
        }
        else
        {
            bz[10] = 0;
        }

        // 右后膝关节关节
        da = BACK_KNEE_OFFSET - dat->q_knee[2];
        a  = copysign(0.01f, da);
        if(fabs(da) > 0.01f)
        {
            setJointCommand(cmd, 2, 3, dat->q_knee[2] + a, 0, 0, testKp, testKd);
        }
        else
        {
            bz[11] = 0;
        }

        uint8_t s = bz[0];
        for(uint8_t i = 1; i < 12; i++)
        {
            s += bz[i];
            // printf("bz[%d] = %d; ", i, bz[i]);
        }
        // printf("s = %d\n", s);

        if(s == 0)
        {
            isNewTest = false;
        }
    }
    else
    {
        stepCnt++;
        stepCnt %= TOTALSTEPS;

        float angle = computeNextAngleWithEp(FRONT_ABAD_OFFSET, 0, stepCnt, TOTALSTEPS, HOLDSTEPS);
        if(angle > cmd->q_des_abad[1])
        {
            setJointCommand(cmd, 1, 1, angle, 0, 0, testKp, testKd);
        }
        else
        {
            setJointCommand(cmd, 1, 1, angle, -0, -0, testKp, testKd);
        }
        if(-angle > cmd->q_des_abad[0])
        {
            setJointCommand(cmd, 0, 1, -angle, 0, 0, testKp, testKd);
        }
        else
        {
            setJointCommand(cmd, 0, 1, -angle, -0, -0, testKp, testKd);
        }

        angle = computeNextAngleWithEp(FRONT_HIP_OFFSET, 0.7, stepCnt, TOTALSTEPS, HOLDSTEPS);
        if(-angle > cmd->q_des_hip[1])
        {
            setJointCommand(cmd, 1, 2, -angle, 0, 0, testKp, testKd);
        }
        else
        {
            setJointCommand(cmd, 1, 2, -angle, -0, -0, testKp, testKd);
        }
        if(-angle > cmd->q_des_hip[0])
        {
            setJointCommand(cmd, 0, 2, -angle, 0, 0, testKp, testKd);
        }
        else
        {
            setJointCommand(cmd, 0, 2, -angle, -0, -0, testKp, testKd);
        }

        angle = computeNextAngleWithEp(FRONT_KNEE_OFFSET * KNEE_K, 1.65f, stepCnt, TOTALSTEPS, HOLDSTEPS);
        if(angle > cmd->q_des_knee[1])
        {
            setJointCommand(cmd, 1, 3, angle, 0, 0, testKp, testKd);
        }
        else
        {
            setJointCommand(cmd, 1, 3, angle, -0, -0, testKp, testKd);
        }
        if(angle > cmd->q_des_knee[0])
        {
            setJointCommand(cmd, 0, 3, angle, 0, 0, testKp, testKd);
        }
        else
        {
            setJointCommand(cmd, 0, 3, angle, -0, -0, testKp, testKd);
        }

        angle = computeNextAngleWithEp(BACK_ABAD_OFFSET, 0, stepCnt, TOTALSTEPS, HOLDSTEPS);
        if(angle > cmd->q_des_abad[3])
        {
            setJointCommand(cmd, 3, 1, angle, 0, 0, testKp, testKd);
        }
        else
        {
            setJointCommand(cmd, 3, 1, angle, -0, -0, testKp, testKd);
        }
        if(-angle > cmd->q_des_abad[2])
        {
            setJointCommand(cmd, 2, 1, -angle, 0, 0, testKp, testKd);
        }
        else
        {
            setJointCommand(cmd, 2, 1, -angle, -0, -0, testKp, testKd);
        }

        angle = computeNextAngleWithEp(BACK_HIP_OFFSET, 0.7, stepCnt, TOTALSTEPS, HOLDSTEPS);
        if(-angle > cmd->q_des_hip[3])
        {
            setJointCommand(cmd, 3, 2, -angle, 0, 0, testKp, testKd);
        }
        else
        {
            setJointCommand(cmd, 3, 2, -angle, -0, -0, testKp, testKd);
        }
        if(-angle > cmd->q_des_hip[2])
        {
            setJointCommand(cmd, 2, 2, -angle, 0, 0, testKp, testKd);
        }
        else
        {
            setJointCommand(cmd, 2, 2, -angle, -0, -0, testKp, testKd);
        }

        angle = computeNextAngleWithEp(BACK_KNEE_OFFSET * KNEE_K, 1.65f, stepCnt, TOTALSTEPS, HOLDSTEPS);
        if(angle > cmd->q_des_knee[3])
        {
            setJointCommand(cmd, 3, 3, angle, 0, 0, testKp, testKd);
        }
        else
        {
            setJointCommand(cmd, 3, 3, angle, -0, -0, testKp, testKd);
        }
        if(angle > cmd->q_des_knee[2])
        {
            setJointCommand(cmd, 2, 3, angle, 0, 0, testKp, testKd);
        }
        else
        {
            setJointCommand(cmd, 2, 3, angle, -0, -0, testKp, testKd);
        }
    }

    cmd->flags[0] = 1;
    cmd->flags[1] = 1;// OK
    cmd->flags[2] = 1;
    cmd->flags[3] = 1;// OK
}

#ifdef VIEW_MODE
int main()
{
    spi_data_t* data = get_spi_data();
    init_spi();

    while(1)
    {
        spi_driver_run();

        printf("\033[H\033[J");// 清屏
        printf("spi_data_t:\n");
        for(int i = 0; i < 4; i++)
        {
            printf("abad: %f, hip: %f, knee: %f, flags: %d\n", data->q_abad[i], data->q_hip[i], data->q_knee[i], data->flags[i]);
            // printf("abad[%d] a: %f, w: %f, t: %f, status: %08X, flag: %d\n", i, data->q_abad[i], data->qd_abad[i], data->tau_abad[i], data->status_abad[i], data->flags[i]);
            // printf("hip[%d]  a: %f, w: %f, t: %f, status: %08X, flag: %d\n", i, data->q_hip[i], data->qd_hip[i], data->tau_hip[i], data->status_hip[i], data->flags[i]);
            // printf("knee[%d] a: %f, w: %f, t: %f, status: %08X, flag: %d\n", i, data->q_knee[i], data->qd_knee[i], data->tau_knee[i], data->status_knee[i], data->flags[i]);
        }
        printf("\n");
        usleep(1500);
    }

    return 0;
}
#elif defined(ZPOSE_MODE)
int main()
{
    spi_command_t* command     = get_spi_command();
    spi_data_t*    data        = get_spi_data();

    init_spi();

    command->flags[0] = 4;
    command->flags[1] = 4;
    command->flags[2] = 4;
    command->flags[3] = 4;

    while(1)
    {
        // printf("\033[H\033[J");// 清屏

        
        spi_driver_run();

        printf("spi_data_t:\n");
        for(int i = 0; i < 4; i++)
        {
            // printf("abad: %f, hip: %f, knee: %f, flags: %d\n", data->q_abad[i], data->q_hip[i], data->q_knee[i], data->flags[i]);
            printf("abad[%d] a: %f, w: %f, t: %f, status: %08X, flag: %d\n", i, data->q_abad[i], data->qd_abad[i], data->tau_abad[i], data->status_abad[i], data->flags[i]);
            printf("hip[%d]  a: %f, w: %f, t: %f, status: %08X, flag: %d\n", i, data->q_hip[i], data->qd_hip[i], data->tau_hip[i], data->status_hip[i], data->flags[i]);
            printf("knee[%d] a: %f, w: %f, t: %f, status: %08X, flag: %d\n", i, data->q_knee[i], data->qd_knee[i], data->tau_knee[i], data->status_knee[i], data->flags[i]);
        }
        printf("\n");

        // if(data->flags[0] == 4 && data->flags[1] == 4 && data->flags[2] == 4 && data->flags[3] == 4)
        if(bitRead(data->flags[0], 3) && bitRead(data->flags[1], 3) && bitRead(data->flags[2], 3) && bitRead(data->flags[3], 3))
        {
            printf("setZeroDone ***********************************************\n");
        }

        usleep(1500);
    }

    return 0;
}
#elif defined(TEST_MODE)
int main()
{
    spi_command_t* command     = get_spi_command();
    spi_data_t*    data        = get_spi_data();
    bool           setZeroDone = false;

    init_spi();

    while(1)
    {
        // printf("\033[H\033[J");// 清屏

        if(setZeroDone)
        {
            standSquatTest(command, data);
        }

        printf("spi_command_t:\n");
        for(int i = 0; i < 4; i++)
        {
            printf("abad a: %f, w: %f, t: %f, kp: %f, kd: %f, flag: %d\n", command->q_des_abad[i], command->qd_des_abad[i], command->tau_abad_ff[i], command->kp_abad[i], command->kd_abad[i], command->flags[i]);
            printf("hip  a: %f, w: %f, t: %f, kp: %f, kd: %f, flag: %d\n", command->q_des_hip[i], command->qd_des_hip[i], command->tau_hip_ff[i], command->kp_hip[i], command->kd_hip[i], command->flags[i]);
            printf("knee a: %f, w: %f, t: %f, kp: %f, kd: %f, flag: %d\n", command->q_des_knee[i], command->qd_des_knee[i], command->tau_knee_ff[i], command->kp_knee[i], command->kd_knee[i], command->flags[i]);
        }
        printf("\n");

        spi_driver_run();

        printf("spi_data_t:\n");
        for(int i = 0; i < 4; i++)
        {
            // printf("abad: %f, hip: %f, knee: %f, flags: %d\n", data->q_abad[i], data->q_hip[i], data->q_knee[i], data->flags[i]);
            printf("abad[%d] a: %f, w: %f, t: %f, status: %08X, flag: %d\n", i, data->q_abad[i], data->qd_abad[i], data->tau_abad[i], data->status_abad[i], data->flags[i]);
            printf("hip[%d]  a: %f, w: %f, t: %f, status: %08X, flag: %d\n", i, data->q_hip[i], data->qd_hip[i], data->tau_hip[i], data->status_hip[i], data->flags[i]);
            printf("knee[%d] a: %f, w: %f, t: %f, status: %08X, flag: %d\n", i, data->q_knee[i], data->qd_knee[i], data->tau_knee[i], data->status_knee[i], data->flags[i]);
        }
        printf("\n");

        // if(data->flags[0] == 4 && data->flags[1] == 4 && data->flags[2] == 4 && data->flags[3] == 4)
        if(bitRead(data->flags[0], 2) && bitRead(data->flags[1], 2) && bitRead(data->flags[2], 2) && bitRead(data->flags[3], 2))
        {
            setZeroDone = true;
        }

        usleep(1500);
    }

    return 0;
}
#else
#error "请选择模式：VIEW_MODE/观察 或 ZPOSE_MODE/设置零位 或 TEST_MODE/测试"
#endif
